#include <SoftwareSerial.h>
#include <math.h>
#define rxPin 10  // pin 3 connects to smcSerial TX
#define txPin 11  // pin 4 connects to smcSerial RX
const int frontLeft  =1;
const int rearLeft   =2;
const int frontRight =3;
const int rearRight  =4;
SoftwareSerial smcSerial = SoftwareSerial(rxPin, txPin);

int numOfMotors=4;
//change to number of attached motors 

//------------------------------------------>setup<---------------------------------------------------------------------
void setup()
{
  // initialize software serial object with baud rate of 19.2 kbps
  smcSerial.begin(19200);
  Serial.begin(9600);
  pinMode(6, OUTPUT);                                                   // CHANGED
  digitalWrite(6, LOW);
  pinMode(6,INPUT);
  
  pinMode(7, OUTPUT);
  digitalWrite(7, LOW);
  pinMode(7, INPUT);
 
  pinMode(8, OUTPUT);                                                   // CHANGED
  digitalWrite(8, LOW);
  pinMode(8,INPUT);
  
  pinMode(9, OUTPUT);                                                   // CHANGED
  digitalWrite(9, LOW);
  pinMode(9,INPUT);
  // the Simple Motor Controller must be running for at least 1 ms
  // before we try to send serial data, so we delay here for 5 ms
  delay(5);
 
  // if the Simple Motor Controller has automatic baud detection
  // enabled, we first need to send it the byte 0xAA (170 in decimal)
  // so that it can learn the baud rate
  smcSerial.write(0xAA);  // send baud-indicator byte
  // next we need to send the Exit Safe Start command, which
  // clears the safe-start violation and lets the motor run
  exitSafeStart();  // clear the safe-start violation and let the motor run
}
//----------------------------------------------------------------------------------------------------------------------
//-------------------------------------->exitSafeStart<-----------------------------------------------------------------
void exitSafeStart()
{
  // must be called when controller restarts and after any error
  int numOfDevices=1;
  //iterates for each attached motor
  for(numOfDevices; numOfDevices <= numOfMotors; numOfDevices++){
    smcSerial.write(0xAA);
    smcSerial.write((byte)numOfDevices);
    smcSerial.write(0x03);
  }
}
//----------------------------------------------------------------------------------------------------------------------
//-------------------------------------->setMotorSpeed<-----------------------------------------------------------------
void setMotorSpeed(int speed, const int deviceNumber)
{
 // speed should be a number from -3200 to 3200
    smcSerial.write(0xAA);
    smcSerial.write(deviceNumber);
    if (speed < 0)
    {
      smcSerial.write(0x06);  // motor reverse command
      speed = -speed;  // make speed positive
    }
    else
    {
      smcSerial.write(0x05);  // motor forward command
    }
    smcSerial.write(speed & 0x1F);
    smcSerial.write(speed >> 5);
}
//--------------------------------------->Read_Status<------------------------------------------------------------------
//Added- not tested
void read_status(int deviceNumber)
{
  int error;
  int targetSpeed;
  int currentSpeed;
  int voltage;
  boolean errorFound=0;
  
   error=getVariable(0,deviceNumber);
   if(error<0){
     Serial.print(error);
     Serial.println("Error");
     errorFound = 1;
   }
   
   if(errorFound != 1){
     targetSpeed= getVariable(20,deviceNumber); 
     if(targetSpeed<0){ 
      Serial.println(targetSpeed);
      Serial.println("target speed");
      errorFound = 1; 
     }
     if(targetSpeed>32767){
       targetSpeed= targetSpeed - 65536; 
     }
   }
   if(errorFound != 1){
     currentSpeed=getVariable(21,deviceNumber);
     if(currentSpeed<0){
      Serial.println(currentSpeed);
      Serial.println("currentspeed");
      errorFound = 1;   
     }
     if(currentSpeed>32767){
        currentSpeed = currentSpeed-65536 ;    
     }
   }
   
   if(errorFound != 1){
   Serial.print('|');
   Serial.print(error, DEC);
   Serial.print('|');
   Serial.print(targetSpeed, DEC);
   Serial.print('|');
   Serial.print(currentSpeed, DEC);
   Serial.println('/');
   }
  /*
  Serial.print(160000, DEC);
   Serial.print('|');
   Serial.print(3200, DEC);
   Serial.print('|');
   Serial.print(3200, DEC);
   Serial.print('|');
   Serial.print(12609, DEC);
   Serial.print('/');
   */
}
//----------------------------------------------------------------------------------------------------------------------
//--------------------------------------->getVariable<------------------------------------------------------------------
signed int getVariable(int variableID, int devicenum)
{
  int value;
  smcSerial.write(0xAA);
  delay(2);
  smcSerial.write(devicenum);
  delay(2);
  smcSerial.write(0x21);
  delay(2);
  smcSerial.write(variableID);
  delay(2);
  value  = smcSerial.read();
  delay(2);
  value= value +256 *smcSerial.read();
  return value;
}
//----------------------------------------------------------------------------------------------------------------------
//--------------------------------------->readByte()<-------------------------------------------------------------------
//called by getVariable
int readByte()
{
  char c;
  Serial.println("Yo");
  c=smcSerial.read();
  //if(smcSerial.readBytes(&c, 1) == 0){ return -1; }
  Serial.println("Yo2");
  return (int)c;
}
//----------------------------------------------------------------------------------------------------------------------

//--------------------------------------->stopMotor<--------------------------------------------------------------------
//TEST----------------------------------------------
void stopMotor(){
int currentDevice= 1;
  for(currentDevice; currentDevice <= numOfMotors; currentDevice++){
    smcSerial.write(0xAA);
    smcSerial.write((byte)currentDevice);
    smcSerial.write(0x60);
  }
}
//----------------------------------------------------------------------------------------------------------------------

void loop(){
  int val; // 0 to 9 
  int RorL; //Right or Left
  int NorP;
  int speedPlaceHolder; //reads in value of speed byte by byte
  int totalSpeed = 0; //the total speed to be set to a motor;
  boolean error = false;

  if(Serial.available()){
    val= Serial.read();
   if(val== 'M'){
      delay(10);
    while(Serial.available() >0){
      totalSpeed = 0;
      RorL=Serial.read();
      NorP=Serial.read();
        for(int count=3;  count >= 0; count--){
         speedPlaceHolder = Serial.read()- '0';
         if(speedPlaceHolder < 0 || speedPlaceHolder > 9){
           Serial.print(speedPlaceHolder);
           error = true;
           break;                                                                 //CHANGED
         }
           totalSpeed+=(pow(10,count))*speedPlaceHolder;
        }
        
      if(NorP == '-'){
        totalSpeed = totalSpeed*-1;
      }
      if(totalSpeed >= 3200){
        totalSpeed = 3200;
      }
      if(totalSpeed <= -3200){
        totalSpeed = -3200; 
      }
    //IF A RANDOM OR MISPLACED BIT OCCURS READ IN EXCESS DATA?
    //INSERT IF GREATER THAN 3200 STOP
      if(!error){                                                //Changed 3/3
        if(RorL == 'L'){
          setMotorSpeed(totalSpeed, frontLeft);
          setMotorSpeed(totalSpeed, rearLeft);
          Serial.print("Left Wheels ");
          Serial.println(totalSpeed);
        }else if(RorL == 'R'){
          Serial.print("Right Wheels  ");
          Serial.println(totalSpeed);
          setMotorSpeed(totalSpeed, frontRight);
          setMotorSpeed(totalSpeed, rearRight);
        }
      }else{
       Serial.print("Error Found! Rover Stopping!");
        setMotorSpeed(0, frontLeft);
        setMotorSpeed(0, frontRight);
        setMotorSpeed(0, rearLeft);
        setMotorSpeed(0, rearRight);
      }
   }
   }else if(val == 'W'){
      read_status(1);
   }else if(val == 'X'){
      read_status(2);
   }else if(val == 'Y'){
      read_status(3);
   }else if(val == 'Z'){
      read_status(4);
   }else if(val == 'E'){
      setMotorSpeed(0,frontLeft);
      setMotorSpeed(0,frontRight);
      setMotorSpeed(0,rearLeft);
      setMotorSpeed(0,rearRight);
      smcSerial.write(0xAA);  // send baud-indicator byte
      Serial.println("Exit safe start");
      exitSafeStart();
   }else if(val == 'K'){                                                      //ADDED 2/21
     stopMotor();
   }else if(val == 'X'){
      pinMode(5, OUTPUT);                                                   // ADDED 2/21
      digitalWrite(5, LOW);
      pinMode(5,INPUT);
      delay(5);
      pinMode(6, OUTPUT);
      digitalWrite(6, LOW);
      pinMode(6, INPUT);
      delay(5);
      pinMode(7, OUTPUT);                                                   // ADDED 2/21
      digitalWrite(7, LOW);
      pinMode(7,INPUT);
      delay(5);
      pinMode(8, OUTPUT);
      digitalWrite(8, LOW);
      pinMode(8, INPUT);
      delay(5);
   }
   
  }
}
